<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=utf-8">
<title>par_kinematics: /home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/deltarobot.cpp Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>

<table border="0">
<tr><td valign="top" style="border-right: 1px solid #333; padding-right: 10px;" >
<p>
Project: <a href="http://ros.org/wiki/par_kinematics">par_kinematics</a>
</p>
<p>
License: BSD
</p>
<p>
Dependencies: 
<ul>
  <li>std_msgs</li>
  <li>roscpp</li>
  <li>par_trajectory_planning</li>
</ul>
</p>
<p>
Used by:
None<br />
</p>
<p>
<a href="../../index.html">All Packages</a>
</p>

</div>
</td><td valign="top" style="padding-left: 10px;" >
<!-- Generated by Doxygen 1.7.1 -->
<div class="navigation" id="top">
  <div class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
    </ul>
  </div>
  <div class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<h1>/home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/deltarobot.cpp</h1>  </div>
</div>
<div class="contents">
<a href="deltarobot_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;<a class="code" href="deltarobot_8h.html">par_kinematics/deltarobot.h</a>&gt;</span>
<a name="l00008"></a>00008 
<a name="l00009"></a><a class="code" href="classDeltaRobot.html#a483dde020bcc6b759ac83efa29bc0e69">00009</a> <a class="code" href="classDeltaRobot.html#a483dde020bcc6b759ac83efa29bc0e69" title="Constructor for deltarobot.">DeltaRobot::DeltaRobot</a>(<span class="keywordtype">double</span> hipLength, <span class="keywordtype">double</span> ankleLength, <span class="keywordtype">double</span> baseSize, <span class="keywordtype">double</span> effectorSize) 
<a name="l00010"></a>00010 {
<a name="l00011"></a>00011                 <a class="code" href="classDeltaRobot.html#a931a360f92077bf197364bbc4eba287e">hip</a> = hipLength;
<a name="l00012"></a>00012                 <a class="code" href="classDeltaRobot.html#afaea16e8df17cf56ae568f6b817f0cc9">ankle</a> = ankleLength;
<a name="l00013"></a>00013                 <a class="code" href="classDeltaRobot.html#adb62b57597123a9448a7ee4d8a17fec8">base</a> = baseSize;
<a name="l00014"></a>00014                 <a class="code" href="classDeltaRobot.html#a06ed6718a14e5747550ad5ac6276b2c3">effector</a> = effectorSize;
<a name="l00015"></a>00015                 <a class="code" href="classDeltaRobot.html#aaad4dd5537c21e1906a698e66e6a3a95">effLoc</a> = <span class="keyword">new</span> <a class="code" href="classPoint.html">Point</a>(0,0,0);
<a name="l00016"></a>00016     
<a name="l00017"></a>00017                 <a class="code" href="classDeltaRobot.html#a4e6286cb0a38b1fb404d052d75d14b0d" title="Method for creating new delta robot arms.">newArms</a>();
<a name="l00018"></a>00018 }
<a name="l00019"></a>00019   
<a name="l00020"></a><a class="code" href="classDeltaRobot.html#a4e6286cb0a38b1fb404d052d75d14b0d">00020</a> <span class="keywordtype">void</span> <a class="code" href="classDeltaRobot.html#a4e6286cb0a38b1fb404d052d75d14b0d" title="Method for creating new delta robot arms.">DeltaRobot::newArms</a>() 
<a name="l00021"></a>00021 {
<a name="l00022"></a>00022         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 3; i++) {
<a name="l00023"></a>00023                 <a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[i]   = <span class="keyword">new</span> <a class="code" href="classArmModel.html">ArmModel</a>(i*120, <a class="code" href="classDeltaRobot.html#a931a360f92077bf197364bbc4eba287e">hip</a>, <a class="code" href="classDeltaRobot.html#afaea16e8df17cf56ae568f6b817f0cc9">ankle</a>, <a class="code" href="classDeltaRobot.html#adb62b57597123a9448a7ee4d8a17fec8">base</a>, <a class="code" href="classDeltaRobot.html#a06ed6718a14e5747550ad5ac6276b2c3">effector</a>);
<a name="l00024"></a>00024                 <a class="code" href="classDeltaRobot.html#a16900ee3d373338644a956860303bda1">arm</a>[i] = <span class="keyword">new</span> <a class="code" href="classDeltaArm.html">DeltaArm</a>(<a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[i]);
<a name="l00025"></a>00025         }
<a name="l00026"></a>00026 
<a name="l00027"></a>00027         <a class="code" href="classDeltaRobot.html#a003ac461d7ad7ce24a9216970a98a611">servoMin</a> = <a class="code" href="classUtil.html#a96a574ae5971d45cc92cbc19677cbe5d" title="Function for converting degrees to radians.">Util::rad</a>(-6.5);
<a name="l00028"></a>00028         <a class="code" href="classDeltaRobot.html#a4b5da4b34859948b1cb2364f81b4e765">servoMax</a> = 0;
<a name="l00029"></a>00029 }
<a name="l00030"></a>00030   
<a name="l00031"></a><a class="code" href="classDeltaRobot.html#a1b84447cde9a3c8a8807517e3b052f4f">00031</a> <span class="keywordtype">bool</span> <a class="code" href="classDeltaRobot.html#a1b84447cde9a3c8a8807517e3b052f4f" title="Method for checking the end effector.">DeltaRobot::isEffectorOk</a>() 
<a name="l00032"></a>00032 {
<a name="l00033"></a>00033         
<a name="l00034"></a>00034         <span class="keywordflow">return</span> !(<a class="code" href="classUtil.html#add5e057769841913daa68836e6fd2790" title="Function for checking validity of a number.">Util::isNaN</a>(<a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[0]-&gt;angle) || <a class="code" href="classUtil.html#add5e057769841913daa68836e6fd2790" title="Function for checking validity of a number.">Util::isNaN</a>(<a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[1]-&gt;angle) || <a class="code" href="classUtil.html#add5e057769841913daa68836e6fd2790" title="Function for checking validity of a number.">Util::isNaN</a>(<a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[2]-&gt;angle));
<a name="l00035"></a>00035 }
<a name="l00036"></a>00036   
<a name="l00037"></a><a class="code" href="classDeltaRobot.html#aed23f0594a258fad5176651242119fc6">00037</a> <span class="keywordtype">bool</span> <a class="code" href="classDeltaRobot.html#aed23f0594a258fad5176651242119fc6" title="Method for moving the delta robot to a specified point.">DeltaRobot::moveto</a>(<a class="code" href="classPoint.html">Point</a> goal) 
<a name="l00038"></a>00038 {
<a name="l00039"></a>00039         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 3; i++) {
<a name="l00040"></a>00040                 <a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[i]-&gt;<a class="code" href="classArmModel.html#a8b0321e28d01ac19423d028756b39606">moveto</a>(goal.<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>, goal.<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>, goal.<a class="code" href="classPoint.html#a05ba3b1dfcb19430582ae953cbbfbded">z</a>);
<a name="l00041"></a>00041                 <a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[i] = <a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[i]-&gt;<a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a>;
<a name="l00042"></a>00042                 <a class="code" href="classDeltaRobot.html#afffffef6e172b447a3f679412c60958a">pangle</a>[i] = <a class="code" href="classDeltaRobot.html#a6425a5ad096a98545332feadc9e485cb">m</a>[i]-&gt;<a class="code" href="classArmModel.html#a62c37ad0350bc177c9c008ac8fcbb875">pangle</a>;
<a name="l00043"></a>00043         }
<a name="l00044"></a>00044 
<a name="l00045"></a>00045         <a class="code" href="classDeltaRobot.html#aaad4dd5537c21e1906a698e66e6a3a95">effLoc</a>-&gt;<a class="code" href="classPoint.html#a7cb951c76a0c975fa06c5a6d76745156">moveto</a>(goal);
<a name="l00046"></a>00046 
<a name="l00047"></a>00047         <a class="code" href="classDeltaRobot.html#a003ac461d7ad7ce24a9216970a98a611">servoMin</a> = std::min( std::min(<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[0], <a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[1]), <a class="code" href="classDeltaRobot.html#a003ac461d7ad7ce24a9216970a98a611">servoMin</a>); 
<a name="l00048"></a>00048         <a class="code" href="classDeltaRobot.html#a003ac461d7ad7ce24a9216970a98a611">servoMin</a> = std::min(<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[2], <a class="code" href="classDeltaRobot.html#a003ac461d7ad7ce24a9216970a98a611">servoMin</a>);
<a name="l00049"></a>00049         <a class="code" href="classDeltaRobot.html#a4b5da4b34859948b1cb2364f81b4e765">servoMax</a> = std::max( std::max(<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[0], <a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[1]), <a class="code" href="classDeltaRobot.html#a4b5da4b34859948b1cb2364f81b4e765">servoMax</a>); 
<a name="l00050"></a>00050         <a class="code" href="classDeltaRobot.html#a4b5da4b34859948b1cb2364f81b4e765">servoMax</a> = std::max(<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[2], <a class="code" href="classDeltaRobot.html#a4b5da4b34859948b1cb2364f81b4e765">servoMax</a>);
<a name="l00051"></a>00051 
<a name="l00052"></a>00052         <span class="keywordflow">return</span> <a class="code" href="classDeltaRobot.html#a1b84447cde9a3c8a8807517e3b052f4f" title="Method for checking the end effector.">isEffectorOk</a>();
<a name="l00053"></a>00053 }
</pre></div></div>
</div>
</td></tr></table>

<hr size="1"><div style="align: right;">
<a href="http://ros.org/wiki/par_kinematics">par_kinematics</a><br />
Author(s): Wouter van Teijlingen</br />
<small>autogenerated on Mon Jun 13 2011 17:06:55</small>
</div>
</body>
</html>
